#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

ros::Subscriber image_subscriber;
ros::Publisher  image_publisher;

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  std::cout<<"Received image! Start to extract feature."<<std::endl;
  try
  {
    // 将ROS图像消息转换为OpenCV图像
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    cv::Mat image = cv_ptr->image;

    // 创建ORB特征提取器
    cv::Ptr<cv::Feature2D> orb = cv::ORB::create();

    // 创建LSD线段检测器
    cv::Ptr<cv::LineSegmentDetector> lsd = cv::createLineSegmentDetector(cv::LSD_REFINE_ADV);

    // 提取ORB特征点
    std::vector<cv::KeyPoint> keypoints;
    orb->detect(image, keypoints);

    // 提取LSD线段
    std::vector<cv::Vec4f> lines;
    cv::Mat grayImage;
    cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY);
    lsd->detect(grayImage, lines);

    // // 在图像上绘制特征点和线段
    cv::drawKeypoints(image, keypoints, image, cv::Scalar::all(-1), cv::DrawMatchesFlags::DEFAULT);
    lsd->drawSegments(image, lines);

    // 发布带有特征点和线段的图像到另一个ROS话题
    cv_bridge::CvImage cv_image;
    cv_image.header = msg->header;
    cv_image.encoding = sensor_msgs::image_encodings::BGR8;
    cv_image.image = image;
    sensor_msgs::ImagePtr image_msg = cv_image.toImageMsg();
    image_publisher.publish(image_msg);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "feature_extraction_node");
  ros::NodeHandle nh("~");
  std::string inputTopic = "";
  std::string outputTopic = "";
  nh.getParam("input_topic", inputTopic);
  nh.getParam("output_topic", outputTopic);
  std::cout<<"Ros node start, param: "<<inputTopic<<" and "<<outputTopic<<std::endl;
  image_subscriber = nh.subscribe(inputTopic, 1, imageCallback);
  image_publisher = nh.advertise<sensor_msgs::Image>(outputTopic, 1);

  ros::spin();

  return 0;
}